using DistributedGISA, Plots, DelimitedFiles

# state constraint
Xc = [@interval(-5,5), @interval(-5,5), @interval(-5,5)]
# number of divisions per dimension
d = 32
# discretized space for centralized system
dsc = DiscreteSpace(Xc,[d,d,d])
# initial approximation of the CIS
Rkall = [[i,j] for i in 1:d for j in 1:d]

# subsystem 1 model and constraints
X1 = [@interval(-5,5), @interval(-5,5)]
ds1 = DiscreteSpace(X1,[d,d])
f1(x,u) = (x[1]*x[1] + u[1], x[1] + 1.0*x[2]*x[2])
u = [@interval(-1,1)]
R1k = [[i,j] for i in 1:d for j in 1:d]

# subsystem 2 model and constraints
X2 = [@interval(-5,5), @interval(-5,5)]
ds2 = DiscreteSpace(X2,[d,d])
f2(x,xi,u) = (xi[1] + x[1]*x[1], x[1] + x[2]*x[2])
R2k = [[i,j] for i in 1:d for j in 1:d]

# subsystem 1 computation
G1 = create_graph(R1k, ds1, u, f1)
nlc1 = select_nonleaving_cells(G1)
lc1 = setdiff(collect(1:length(R1k)))
R1k = R1k[nlc1]
R1 = create_boxes(R1k,ds1)
writedlm("R1_nonlinear.txt",unique!(vcat([collect(vertices(b)) for b in R1]...)))

R1tt = select_test_cells(nlc1,lc1,G1,d,Rkall) 

# subsystem 2 computation
G2 = create_graph(R2k, ds2, R1k, ds1, u, f2)
nlc2 = select_nonleaving_cells(G2)
lc2 = setdiff(collect(1:length(R2k)))
R2k = R2k[nlc2]
R2 = create_boxes(R2k,ds2)
writedlm("R2_nonlinear.txt",unique!(vcat([collect(vertices(b)) for b in R2]...)))

# lc2 = [i for (i,x) in enumerate(outdegree(G2)) if x<=50]
R2tt = select_test_cells(nlc2,lc2,G2,d,Rkall)

# extract R3k
R3k = [[i,k] for (i,j1) in R1k for (j2,k) in R2k if j1==j2]
R3 = create_boxes(R3k,ds2)

writedlm("R3_nonlinear.txt",unique!(vcat([collect(vertices(b)) for b in R3]...)))
# select test cells
# Rtt = select_test_cells(nlc2,G2,d,Rkall)

# reconstruct sets
# reconstruct full system solution from subsystem solutions
Rr = unique!([[i,j1,k] for (i,j1) in R1k for (j2,k) in R2k if j1==j2])

# reconstruct test cells for testings
Rt = unique!([[i,j,k] for (i,j,k) in Rr if [i,j] in R1tt || [j,k] in R2tt])

# full system model for validation
f(x,u) = (1.0*x[1]*x[1] + u[1], x[1] + 1.0*x[2]*x[2], x[2] + 1.0*x[3]*x[3])
Rkr = validate_set(Rr, Rt, dsc, u, f)

# save data
bb = create_boxes(Rkr,dsc)
writedlm("R_nonlinear.txt",unique!(vcat([collect(vertices(b)) for b in bb]...)))
# while test

# end
println("Done!!!")

# end
# arr = ConvexHullArray(bxs)
# ak1 = Projection(arr,[1,2])
# ak2 = Projection(arr,[2,3])

# for plotting
# R1c = 